/*
 * To change this license header, choose License Headers in Project Properties.
 * To change this template file, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.wpi.first.wpilibj.templates.subsystems;

import edu.wpi.first.wpilibj.Relay;
import edu.wpi.first.wpilibj.Servo;
import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.templates.OI;
import edu.wpi.first.wpilibj.templates.RobotMap;
import edu.wpi.first.wpilibj.templates.commands.BallFeederWithJoystick;

/**
 *
 * @author Robotics
 */
public class BallFeeder extends Subsystem {
    // Put methods for controlling this subsystem
    // here. Call these from Commands.
    private Relay feederMotor;
    public Servo leftServo;
    public Servo rightServo;
    
    //Angle constants
    public final double SERVO_START_ANGLEL = 90.0;
    private final double SERVO_END_ANGLEL = 0.0;//IN DEGREES
    public final double SERVO_START_ANGLER = 90.0;
    private final double SERVO_END_ANGLER = 180.0;//IN DEGREES
    
    //Feeder constants
    /*private final double FEEDER_CW = 1.0;
    private final double FEEDER_CCW = -1.0;
    private final double FEEDER_STOP = 0.0;
    */        

    public BallFeeder(){
        
        feederMotor = new Relay(RobotMap.FEEDER_SPIKE_RELAY);
        leftServo = new Servo(RobotMap.LEFT_SERVO_PWM);
        rightServo = new Servo(RobotMap.RIGHT_SERVO_PWM);
    }
    
    public void initDefaultCommand() {
        // Set the default command for a subsystem here.
        //setDefaultCommand(new MySpecialCommand());
        setDefaultCommand(new BallFeederWithJoystick());
        
    }
    
    public void servoLeftDrop(){
        leftServo.setAngle(SERVO_END_ANGLEL);
    }
    
    public void servoRightDrop(){
        rightServo.setAngle(SERVO_END_ANGLER);
    }
    
    public void servosDrop(){
        servoLeftDrop();
        servoRightDrop();
    }
    
    public void servoLeftReset(){
        leftServo.setAngle(SERVO_START_ANGLEL);
    }
    
    public void servoRightReset(){
        rightServo.setAngle(SERVO_START_ANGLER);
    }
    
    public void servosReset(){
        servoLeftReset();
        servoRightReset();
    }
      
    public double getServoAngleR(){
        return rightServo.getAngle();
    }
    
    public double getServoAngleL(){
        return leftServo.getAngle();
    }
    
    public void motorSpinForward(){
        feederMotor.set(Relay.Value.kForward);
    }
    
    public void motorSpinReverse(){       
        feederMotor.set(Relay.Value.kReverse);
    }
    
    public void motorStop(){
        feederMotor.set(Relay.Value.kOff);
    }
    
    public void motorSpin(double Magnitude){
        if(Magnitude > 0){
            motorSpinForward();
        }else if(Magnitude < 0){
            motorSpinReverse();
        }else{
            motorStop();
        }
    }
}
